#ifndef PObH_H
#define PObH_H

#include <ostream>
#include <vector>


void display_scene(int argc, char* argv[]);
void single_robot_optimization(int argc, char* argv[]);
void single_robot_optimization_graphics(int argc, char* argv[]);


using namespace std;
/*
template<class RefPoint2D> // RefPoint2D should have location and rotation
ostream& operator<<(ostream& os,vector<RefPoint2D> path)
{
	os << "1" << endl;
  long long size=path.size();
  os << to_string(size) << endl;
  for (unsigned int i = 0; i < path.size(); i++)
  {
	   os << path[i] << endl;  
  }  
  return os;
};
*/
template<class RefPoint2D> // RefPoint2D should have location and rotation
ostream& operator<<(ostream& os,vector<RefPoint2D> path)
{
  //os << "1" << endl;
  for (unsigned int i = 1; i < path.size(); i++)
  {
	  RefPoint2D p1 = path[i-1];
	  RefPoint2D p2 = path[i];
	  double x = CGAL::to_double(p2.get_location().x());
	  double y = CGAL::to_double(p2.get_location().y());
	  double a1 = p1.get_rotation().to_angle();
	  double a2 = p2.get_rotation().to_angle();	
	  long double delta = (long double)smallest_angle(a1,a2);
	  
	  /*if (p1.get_location() != p2.get_location())*/{
		  os << "line " << to_string((long double)x);
		  os << " " << to_string((long double)y) << endl;
	  }
	  /*if (delta)*/ os << "angle " << std::to_string(delta) << endl;
  }  
  return os;
};

#endif //PObH_H